/******************************************************************
 * 作者：sipeiliu
 * 邮箱：sipeiliu@163.com
 * 描述：电机模块，L9110S四路直流电机驱动板，TT直流减速电机，麦卡纳姆轮
 * 日期：2023.3.10
 ******************************************************************/

#include "/opt/sighter/inc/motor.h"

#include <wiringPi.h>
#include <pthread.h>

/*使能电机*/
void enable_motor(void)
{
    wiringPiSetup();            // 初始化wiringpi
    for (int i = 0; i < 8; i++) // 配置为输出模式
    {
        pinMode(i, OUTPUT);
    }
}

void motor_run(int way)
{
    /*初始化互斥锁*/
    static pthread_mutex_t mutex;
    pthread_mutex_init(&mutex, NULL);

    /*互斥锁尝试上锁*/
    pthread_mutex_trylock(&mutex);

    /*选择移动方式*/
    if (way == 0)
    {
        int gpio_level[8] = {0, 0, 0, 0, 0, 0, 0, 0}; // 停止
        for (int i = 0; i < 8; i++)
        {
            digitalWrite(i, gpio_level[i]);
        }
    }
    else if (way == 1)
    {
        int gpio_level[8] = {1, 0, 1, 0, 1, 0, 1, 0}; // 前移
        for (int i = 0; i < 8; i++)
        {
            digitalWrite(i, gpio_level[i]);
        }
    }
    else if (way == 2)
    {
        int gpio_level[8] = {0, 1, 0, 1, 0, 1, 0, 1}; // 后移
        for (int i = 0; i < 8; i++)
        {
            digitalWrite(i, gpio_level[i]);
        }
    }
    else if (way == 3)
    {
        int gpio_level[8] = {0, 1, 1, 0, 1, 0, 0, 1}; // 左移
        for (int i = 0; i < 8; i++)
        {
            digitalWrite(i, gpio_level[i]);
        }
    }
    else if (way == 4)
    {
        int gpio_level[8] = {1, 0, 0, 1, 0, 1, 1, 0}; // 右移
        for (int i = 0; i < 8; i++)
        {
            digitalWrite(i, gpio_level[i]);
        }
    }
    else if (way == 5)
    {
        int gpio_level[8] = {1, 0, 0, 1, 1, 0, 0, 1}; // 正转
        for (int i = 0; i < 8; i++)
        {
            digitalWrite(i, gpio_level[i]);
        }
    }
    else if (way == 6)
    {
        int gpio_level[8] = {0, 1, 1, 0, 0, 1, 1, 0}; // 反转
        for (int i = 0; i < 8; i++)
        {
            digitalWrite(i, gpio_level[i]);
        }
    }

    /*互斥锁解锁*/
    pthread_mutex_unlock(&mutex);
}

// /*停止转动，四个轮子都停止*/
// void motor_stop(void)
// {

//     for(int i = 0; i < 8; i++)
//     {
//         digitalWrite(i, gpio_level[i]);
//     }
// }

// /*向前移动，四个轮子都正转*/
// void motor_forward(void)
// {
//     int gpio_level[8] = {1, 0,
//                          1, 0,
//                          1, 0,
//                          1, 0}; // 电机电平
//     for(int i = 0; i < 8; i++)
//     {
//         digitalWrite(i, gpio_level[i]);
//     }
// }

// /*向后移动，四个轮子都反转*/
// void motor_backward(void)
// {
//     int gpio_level[8] = {0, 1,
//                          0, 1,
//                          0, 1,
//                          0, 1}; // 电机电平
//     for(int i = 0; i < 8; i++)
//     {
//         digitalWrite(i, gpio_level[i]);
//     }
// }

// /*向左移动，前左轮反转，前右轮正转，后左轮正转，后右轮反转*/
// void motor_left(void)
// {
//     int gpio_level[8] = {0, 1,
//                          1, 0,
//                          1, 0,
//                          0, 1}; // 电机电平
//     for(int i = 0; i < 8; i++)
//     {
//         digitalWrite(i, gpio_level[i]);
//     }
// }

// /*向右移动，前左轮正转，前右轮反转，后左轮反转，后右轮正转*/
// void motor_right(void)
// {
//     int gpio_level[8] = {1, 0,
//                          0, 1,
//                          0, 1,
//                          1, 0}; // 电机电平
//     for(int i = 0; i < 8; i++)
//     {
//         digitalWrite(i, gpio_level[i]);
//     }
// }

// /*顺时针转动，前左轮正转，前右轮反转，后左轮正转，后右轮反转*/
// void motor_clockwise(void)
// {
//     int gpio_level[8] = {1, 0,
//                          0, 1,
//                          1, 0,
//                          0, 1}; // 电机电平
//     for(int i = 0; i < 8; i++)
//     {
//         digitalWrite(i, gpio_level[i]);
//     }
// }

// /*逆时针转动，前左轮反转，前右轮正转，后左轮反转，后右轮正转*/
// void motor_anticlockwise(void)
// {
//     int gpio_level[8] = {0, 1,
//                          1, 0,
//                          0, 1,
//                          1, 0}; // 电机电平
//     for(int i = 0; i < 8; i++)
//     {
//         digitalWrite(i, gpio_level[i]);
//     }
// }

// void main(void)
// {
//     enable_motor();
//     motor_run(FORWARD);
// }